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ABSTRACT 

The  U.S.  Navy  and  a  number  of  its  contractors  are 
presently  developing  unmanned  miniature  submarines  for 
several  vital  underwater  missions.  These  include  surveil- 
lance, submarine  tracking,  and  bottom  mapping.  Foregoing 
Reserarch  at  NFS  produced  a  "testbed"  as  a  research  platform 
for  demonstrating  the  performance  of  AUVs.  Combining  the 
power  of  an  IBM  PC/AT  in  conjunction  with  a  high  level 
programming  language,  a  state  space  dive  control  system  was 
developed  and  instituted  for  the  3  0  inch  AUV  model. 
Parameter  Estimation  using  a  Recursive  Least  Squares  Fit 
scheme  and  a  State  Observer  were  incorporated  in  the 
controller.  Procedures  dealing  with  hardware/software 
interfacing,  AUV  simulation  analysis,  and  computation  speed 
of  large  programming  code  were  investigated. 
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I.   INTRODUCTION 

A.   GENERAL 

The  crux  of  this  thesis  is  to  develop,  test  and  evaluate 
a  real-time  state-space  derived  dive  plane  controller  for 
the  NFS  prototype  Autonomous  Underwater  Vehicle  (AUV) .  The 
U.S.  Navy  and  a  number  of  its  contractors  are  presently 
developing  unmanned  miniature  submarines  (AUV's)  for  several 
important  underwater  missions.  These  include  surveillance, 
submarine  tracking,  submarine  decoys,  bottom  surveying,  and 
off-board  sensor  support  for  both  surface  ships  and 
submarines  [Ref .  1] .  The  shape  of  these  vehicles  is  similar 
to  that  of  a  large  torpedo  and  can  be  deployed  on 
submarines,  and  surface  ships. 

The  AUV  testbed  at  the  Naval  Postgraduate  School  (NPS) 
currently  being  developed  is  to  examine  the  use  of,  and 
problems  associated  with,   advanced  control  technologies. 
The  objectives  include: 

-  determine  requirements  imposed  by  the  implied  need  for 
artificial  intelligence, 

-  development  of  advanced  computer  control  concepts  and 
architecture , 

-  determine  system  interface  feasibility.   [Ref.  2] 
Unlike  the  conventional  body-of-revolution  design,  the 

NPS  testbed  is  similar  to  a  low  aspect  ratio  wing.   It  is 
similar  conceptually  to  the  late  model  Swimmer  Delivery 


Vehicle  (SDV)  design,  but  actually  based  on  a  two  and  three 
to  one  aspect  ratio  box  shape  vehicle.  Vehicle  selection 
was  based  initially  on  the  availability  and  thoroughness  to 
which  the  vehicle  hydrodynamic  characteristics  were  modeled 
and  validated,  providing  for  ease  of  program  development  and 
verification  [Ref.  3]. 

Modern  trends  in  control  system  design  are  moving 
towards  greater  complexity.  The  desire  for  complex  tasks 
and  greater  accuracy  fuel  the  need  for  high  powered 
computers  to  accomplish  real-time  control  tasks  in  short 
duration.  One  of  the  main  interests  here  is  to  determine 
the  capabilities  of  real-time  control  with  adaptive 
autopilots  and  various  computer  architectures.  The  need  for 
adaptivity  lies  in  the  need  to  achieve  robust  performance 
over  a  wide  range  of  operating  conditions.  Also,  although 
computational  speed  may  suffer,  rapid  response  is  seen  to  be 
important  when  evasive  and/or  obstacle  avoidance  maneuvers 
are  called  for  at  the  supervisory/expert  system  level.  It 
follows  that  the  design  trade-off  between  response  and 
robustness  requires  examination.  For  these  reasons,  this 
particular  work  has  concentrated  on  the  development  of  a 
real-time  controller  including  the  ability  to  perform 
parameter  estimation,  and  the  issues  surrounding  its 
response  time  performance. 


B.   REVIEW  OF  PREVIOUS  WORK 

The  initial  proposal  from  the  Naval  Postgraduate  School 
for  research  on  an  Autonomous  Underwater  Vehicle  began  1 
October  1987.  It  was  titled  "Navigation,  Path  Planning, 
Dynamics  and  Control  of  Generic  Autonomous  Underwater 
Vehicles,"  and  was  submitted  to  the  Naval  Surface  Weapons 
Center  in  Silver  Spring,  Maryland.  The  primary  stated 
objective  was  to  assess  the  capabilities  for  the  design, 
construction,  testing,  and  operation  of  a  testbed  as  a 
research  platform  for  demonstrating  the  performance  of  AUVs 
[Ref.  2]. 

The  first  task  involved  selecting  a  suitable  submersible 
model.  A  non-conventional  wing-shaped  model  with  body  cross 
sections  nearly  rectangular  rather  than  circular  became  the 
selected  choice. 

Using  Dynamic  Simulation  Language  (DSL),  Boncal  (1987) 
modified  the  existing  equations  of  motion  (DTNSRDC  2510)  and 
performed  all  the  necessary  differentials  for  their  lineari- 
zation to  suit  the  needs  of  the  AUV  controller  development. 
The  linearization  was  necessary  to  the  design  of  model-based 
controls  and  to  simulate  controlled  responses  for  all  six 
degrees  of  freedom  of  vehicle  motion  [Ref.  3].  Both  linear 
and  non-linear  models  were  developed  and  evaluated.  This 
lead  to  the  initial  development  of  a  controller  robust 
enough  to  handle  a  variety  of  reflexive  type  maneuvers  over 
a  wide  range  of  speeds  (in  the  programming  language  DSL) . 


In  order  to  investigate  the  real-time  control  issues, 
and  to  provide  a  means  to  generate  experimental  data  for  the 
evaluation  of  parameter  identification  methods,  a  later 
effort  by  Brunner  (1988) ,  set  out  to  design,  build,  and  test 
a  model  3  0  inches  in  length  and  7  inches  wide.  The  model 
was  self-propelled  and  remotely  controlled  via  radio 
transmission.  It  was  equipped  with  rate  gyro  sensors,  a 
pressure  cell  depth  sensor  and  a  pressure  cell  speed  sensor. 
Two  DC  motors  provided  the  propulsion  power  [Ref.  4].  Open- 
loop  dive  plane  dynamic  response  tests  were  performed  in  a 
water  tank  where  response  measurements  of  depth  were  made  by 
pressure  cells,  and  pitchrate  by  a  pitchrate  gyro.  Data 
from  each  run  were  recorded  using  PCLAB  and  the  DT  NOTEBOOK 
[Ref.  5]  data  acquisition  software,  and  analyzed  using  the 
MATRIXx  [Ref.  6]  data  analysis  capability.  Comparing  these 
data  with  the  computer  model  simulation,  hydrodynamic 
coefficients  and  system  transfer  functions  were  developed. 

Initial  developments  of  a  digital  autopilot  to  support 
the  NPS  test  vehicle  were  made  by  Delaplane  (1988)  ,  wherein 
an  IPM  PC/AT  was  used  in  conjunction  with  DATA  TRANSLATION 
Analog  to  Digital  (A/D)  interfacing  boards,  as  the  platform 
for  the  real-time  closed  loop  controller.  A  simplified 
control  program  for  AUV  dive  plane  commands  had  been 
implemented.  Signal  generators  were  used  to  replicate  AUV 
transducer  outputs  of  depth,  speed,  and  pitchrate  to  verify 


system  operation  [Ref.  7].  The  inclusion  of  real  control 
laws,  and  the  system  parameter  identification,  however,  was 
left  to  the  activity  reported  herein. 

C.   SCOPE  OF  WORK 

The  scope  of  this  investigation  was  to  develop  the 
digital  autopilot  [Ref.  7]  into  a  three  state  feedback. 
Single  Input  multiple  Output,  real-time  closed-loop  dive 
control  system  for  the  NFS  AUV  model.  Initial  stages  of 
research  lead  to  the  notion  that  the  pitch-heave  coupling 
prevalent  in  many  underwater  vehicles  was  not  significant 
for  this  vehicle.  The  three  state  variables  of  the  dynamic 
model  were  then  reduced  to  the  pitch  rate,  pitch  angle,  and 
the  water  depth.  Because  of  tank  length  limitations  it  was 
foreseen  that  an  analog  computer  model  of  the  vehicle 
dynamics  would  be  essential  to  the  ease  of  the  real  time 
control  development,  and  therefore,  a  scaled  simulator  was 
built. 

The  final  stages  of  program  development  were  to 
implement  the  digital  dive  plane  controller  compatible  with 
the  30  inch  AUV  model.  Considering  the  small  vehicle  size, 
and  relatively  high  component  costs,  the  NFS  AUV  was  unable 
to  include  a  pitch  angle  sensor.  Since  pitch  angle  feedback 
was  later  shown  to  be  critical  to  the  autopilot  stability, 
where  the  pitch  angle  could  not  be  measured  directly,  a 
state  reconstruction  system  was  needed  and  had  to  be 
designed.    This  led  to  the  development  of  a  full  state 


observer  driven  by  measured  inputs  and  outputs  [Ref.  8]. 
The  Observer,  including  the  identification  of  pitch  rate 
sensor  bias,  the  bias  compensation,  and  its  real  time 
performance  was  large  part  of  this  effort. 

D .   APPROACH 

Primarily,  a  solid  understanding  of  the  software/ 
hardware  used  was  essential.  In  this  control  system,  a  high 
level  programming  language  (Turbo  Pascal)  was  interfaced 
with  an  A/D  translation  board  (hardware)  via  a  software 
package  (PCLAB) .  References  9,  10,  11  established  a 
sufficient  base  on  which  the  elaborate  program  schemes  could 
be  developed  effectively.  Once  accomplished,  the  challenge 
of  debugging  large  programs  could  readily  be  resolved. 

Secondly,  incorporating  experimental  data  from  previous 
AUV  research  proved  paramount.  This  research  thesis  could 
not  be  accomplished  without  the  foundation  set  by  preceding 
studies. 

Lastly,  using  real-time  feedback  of  AUV  model  dynamics 
via  the  analog  computer  strengthened  the  ease  by  which 
program  development  could  proceed.  The  Analog  Computer 
simulated  the  model  dynamics  including  the  sensor  bias  and, 
partially,  the  effects  of  changing  speed.  This  provided  for 
the  preliminary  design  of  a  robust  state-space  controller. 


E.   WHAT  FOLLOWS 

In  Chapter  II,  some  detail  is  provided  concerning  the 
models  of  the  vehicle  dynamics.  Chapter  III  discusses  the 
analog  computer  modelling.  Chapter  IV  gives  an  account  of 
the  program  development.  Chapters  V  and  VI  contain  a 
discussion  of  results  and  Chapter  VII  is  a  summary  and 
conclusion. 


II.   SIMPLIFIED  MODELING  OF  VEHICLE  DIVE  DYNAMICS 

A.  GENERAL 

In  this  chapter,  a  review  of  earlier  experiments 
conducted  under  the  work  of  Brunner  [Ref.  4],  appears  and 
the  details  of  the  third-order  model  of  the  vehicle  is 
given.  This  leads  to  a  discussion  of  the  design  issues  in 
the  controller  and  the  observer  systems  and  the  special 
problem  of  identifying  the  pitch  rate  sensor  bias  from  input 
and  output  data . 

B.  REVIEW  OF  OPEN  LOOP  EXPERIMENTS 

Open  loop  experiments  were  conducted  during  January, 
19  8  8  (Brunner)  ,  in  which  the  model  AUV  was  driven  by  open 
loop  command  to  perform  dive  plane  maneuvers.  From  these 
test  runs,  MATRIXx  datafiles  were  constructed  for  both  the 
command  voltage  to  the  dive  plane  and  the  measured  voltages 
corresponding  to  the  pitch  rate,  and  the  water  depth 
responses.  Using  the  linear  model  time  history  response 
features  in  MATRIXx,  together  with  many  simulation  runs  in 
which  hydrodynamic  parameters  were  varied  both  in  value  and 
in  combinations  of  values,  several  "best  fit"  parameter 
combinations  were  synthesized  to  represent  the  dynamics  of 
the  model  AUV.  It  was  discovered  that  the  influence  of 
coupling  between  the  heave  mode,  and  the  pitch  mode  dynamics 
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was  not  strong  in  determining  the  pitch  rate  behavior,  and 
that  for  the  purposes  of  control  shape  design,  the  heave 
motion  would  be  neglected.  The  influence  of  the  power  cord 
was  found  to  be  a  determining  factor  in  the  overall  heave 
response,  but  future  tests  would  be  conducted  by- 
neutralizing  the  weight  of  the  cord,  and  this  effect  would 
not  adversely  affect  closed  loop  results. 

The  numerical  values  of  the  vehicle  parameters,  with 
their  respective  dimensional  units,  were  established  as 
follows  in  the  next  section. 

C.   THIRD-ORDER  MODEL  FOR  DIVE-PLANE  DYNAMICS 

The  vehicle  equations  of  motion  in  the  dive  plane  are 
the  surge,  heave,  and  pitch  motion  equations  together  with 
the  kinematical  relationships  linking  the  global 
longitudinal  and  vertical  velocity  components  to  the  body 
fixed  surge,  heave,  and  pitch  rates.  These  are  given  in 
detail  in  Boncal  (1987) .  When  linearized  about  a  constant 
speed  straight  line  flight  path,  the  relationships  simplify 
to  four  first-order  state  equations  in  the  heave  and  pitch 
rates  and  positions.   In  short. 
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where  w,   q,  6  ,   z  are  the  four  state  variables  and  the 
parameters  are  given  by, 

HEAVE  DAMPING  ZW  =  -1.5 


PITCH  CROSS  COUPLING 


HEAVE  CROSS  COUPLING 


PITCH  DAMPING 


MOMENT  EFFECT 


MOMENT  EFFECT 


MASS  PLUS  ADDED  MASS 


INERTIA  PLUS  ADDED  MASS 


EFFECT  OF  TETHER 


EFFECT  OF  TETHER 


ZQ  =  0.0 

MW  =  0.0 

MQ  =  -0.15 

ZD  =  0.0 

MD  =  0.22  5 

MZDW  =  1.005 

lYY  =  0.072 

Fl  =  0.11 

F2  =  0.0 


FORWARD  SPEED  UX  =  2.1  FEET/SEC 

LENGTH  L  =  2.5  FEET, 

where  the  units  of  the  parameters  are  given  in  Table  2 . 1 

TABLE  2.1 
PARAMETER  UNITS 


Value 

Value 

Parameter 

Units 

^Dimensional) 

fVolts  Units) 

MQ*UX/L*IYY 

sec"-*- 

10.0  1/sec 

1.8 

UX 

ft/sec 

10.0  ft/sec 

2.1 

MD*Ux2/l2iyY 

rad/sec^ 

10.0  l/sec2 

2.531 
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It  should  be  pointed  out  that  the  matching  of  experimental 
data  was  done  by  comparison  of  sensor  output  voltage 
response  time  history  and  that  proper  conversion  to 
dimensional  values  was  essential  to  being  able  to  translate 
to  control  gains  for  real  time  control  system  design. 

Because  it  was  found  that  the  effects  of  the  heave/pitch 
coupling  were  not  strong,  and  that  the  influence  of  w  was 
dominated  by  the  power  cord,  it  was  assumed  that  for  the 
purpose  of  control  system  design,  the  heave  dynamics  would 
remain  unmodelled,  and  the  model  base  would  be  reduced  to 
third-order  with  the  equations  as  follows: 

X'  =  [q,e,Z], 


X  = 


-1.8  0.0  0.0 
1.0  0.0  0.0 
0.0   -2.1    0.0 


2.531 

0.0 

0.0 


D.   FULL  STATE  FEEDBACK  CONTROL  DESIGN 

For  the  nominal  forward  speed  of  2.1  ft/sec,  the  system 
dynamics  matrix,  and  the  system  input  and  output  matrices 
are: 


A  = 


•1.8  0.0  0.0 
1.0  0.0  0.0 
0.0   -2.1    0.0 


B  = 


2.531 

0.0 

0.0 


C  =  [0.0,0.0,1.0] 
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These  equations  are  characterized  by  the  open  loop 
eigenvalues: 

V  =  [-1.8, 0.0,0.0] 

which  properly  indicates  the  first-order  pole  for  the  pitch 
rate  response  and  the  double  integration  from  pitch  rate  to 
depth.  The  output  matrix  has  diagonal  coefficients  with 
either  unity  values  or  other  constants  depending  on  whether 
the  output  signals  are  considered  to  be  sensor  output 
voltages  or  the  dimensional  values  of  the  system  state 
variables. 

Noting  that  the  open  loop  system  is  completely 
controllable  in  the  formal  sense,  a  pole  placement  technique 
was  determined  to  be  appropriate  for  the  autopilot  design. 
Additionally,  since  the  overall  system  required  a  digital 
controller,  discrete  time  methods  were  needed,  and  the 
question  of  sample  rate  arose. 

Digital  systems  for  the  control  of  continuous  processes 
are  limited  to  providing  a  sequence  of  stepwise  constant 
levels  that  can  be  updated  every  sample  time,  but  remain 
fixed  within  that  interval.  Clearly,  a  short  interval  is 
preferred.  However,  the  execution  time  of  extensive  amounts 
of  code  requires  that  the  sampling  period  must  be  sufficient 
for  code  execution,  but  it  must  not  be  so  short  as  to 
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places  limitations  on  the  closed  loop  poles  as  specified  for 
the  control  design,  being  consistent  with  the  execution  time 
of  the  real  time  code. 

In  balancing  the  foregoing,  two  systems  were  designed. 
The  first,  at  20  Hz,  was  designed  to  accommodate  a  control 
code  that  executed  a  full  three  state  feedback  design  where 
the  system  parameter  identification  was  bypassed.  The 
second  was  at  5  Hz  (the  maximum  upper  limit  possible  under 
the  present  hardware  configuration)  where  parameter 
identification  was  performed. 

The  open  loop  system  dynamics  matrix  and  input  matrix 
were  then  discretized  using  the  20  Hz  sample  rate  so  that 
the  new  A,  and  B  matrices  became: 


A  = 


.9139    0.0      0.0 

.0478    1.0      0.0 

-.0025   -.1050    1.0 


B  = 


.121 
.0031 
-.0001 


Closed  loop  poles  for  the  20  Hz  system  were  then  specified 
at 

s  =  [0.92,0.925,0.926] 

in  the  z-domain,  corresponding  to  three  poles  in  the  s-plane 
with  time  constants  equal  to  1.0  sec,  with  the  following 
state  feedback  gains, 
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Kc*X  =  [0.3066,0.8604,-0.2493], 

where  allowances  were  made  for  the  output  transducer  gain. 
For  a  state  regulator  system,  the  control  law  is, 

u  =  -Kc*x 

when  constant  non-zero  command  signals  are  to  be  introduced, 
the  control  law  requires  modification  to, 

u  =  N*r  -  Kc*x 

in  which  the  command (s)  r  and  the  set  point  scaling  matrix, 
N,  are  specified  so  that  the  tracking  error  r-y  vanishes. 
The  closed  loop  system  equations  become 

X  =  (A-B*Kc) *x+B*N*r;      y  =  C*x 
and  the  tracking  error  vanishes  in  steady  state  when, 

C* (A-B*Kc) "^*B*N=I;  N=(C* (A-B*Kc) ~^*B) 
yielding  the  discrete  time  control  law  used, 
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u(k)  =  -0.2493*[r(k)-z(k) ]-z(k) ] 

-0.8604* [6 (k) ]-0.3  066*[q(k) ] 

This  control  provides  reasonable  response  time  and  calls  for 
a  control  surface  activity  level  that  would  exceed  the 
stroke  of  the  vehicle  dive  plane. 

E.   OBSERVATION  OF  UNMEASURED  STATES 

In  the  development  of  the  model  AUV  the  size  limitation 
prevented  the  inclusion  of  an  angle  sensor  for  pitch,  so  the 
control  system  was  required  to  reconstruct  that  signal  in 
order  to  yield  stable  results.  The  application  of  a  full 
state  observer,  configured  as  a  model  based  compensator  is 
given  by  the  following  one  step  prediction  equations, 

x(k+l)  =  A*x(k)  +  B*u(k)  +  Ko*[y(k)-C*x(k) ] 

in  which  y(k)  is  the  previous  time  sensor  output  signals. 
In  this  case,  two  outputs  were  available  to  the  observer, 
the  pitch  rate,  and  the  water  depth.  The  observer  gains, 
Ko,  may  be  selected  by  using  the  dual  of  the  controller  pole 
placement  technique,  or  by  using  Linear  Quadratic  Regulator 
optimization.  Although  pole  placement  techniques  in  general 
are  based  on  a  single  input  signal,  two  inputs  can  be 
handled  by  the  assignment  of  some  weighting  value  (i.e., 
equal)  ineffective  towards  the  outcome.  The  effect  of  the 
two  signals,  y(k),  now  becomes, 
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y(k)  =  [l;l]*ye(k) 

so  that  the  gains  for  the  two-input  system  become 

Ko  =  [l;l]*ke 

where  Ke  are  found  via  pole  placement  using  ye(k). 

The  inclusion  of  the  observation  (and  later  parameter 
identification)  algorithms  required  the  use  of  a  slower 
sample  rate,  5  Hz.  Consequently,  enclosed  loop  poles  of  the 
observer  were  chosen  as  a  compromise  between  fast  response 
and  the  resulting  sensitivity  to  extraneous  noise  when 
implemented  as  a  model  based  compensator.  The  following 
were  the  final  selection, 

Ko  =  [0.0;-0.4029;0.5677] 

F.   PARAMETER  ESTIMATION  OF  SENSOR  BIAS 

Parameter  identification  of  the  rate  gyro  bias  was 
accomplished  by  the  use  of  an  ARMA  model  of  the  gyro  system 
itself  where  the  input  variable  was  the  dive  plane  angle  and 
the  output  was  pitch  rate.  The  ideal  transfer  function 
between  input  and  output  is  a  first  order  system  which  has 
two  numerator  coefficients,  and  two  denominator  coefficients 
to  identify.  (Note  that  the  z-domain  transfer  function  is 
to  be  used  because  we  are  dealing  with  sequences  of  sampled 
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data.)    With  this  form,  the  parameter  vector  and  the  data 
vector  can  be  written  as, 

e  =  [1  al  bO  bl];   x'  =  [y(t)  y(t-l)  d(t)  d(t-l)]; 


and 


e*x  =  bias(t) . 

The  foregoing  is  then  repeated  and  a  least  squares  fit 
for  6  is  then  obtained  after  the  bias  is  included  inside  the 
vector  6.  Least  squares  solution  of  these  equations  yields 
both  the  vector  of  parameters,  6  ,  and  the  unknown  bias.  In 
this  work,  a  recursive  least  squares  algorithm  was  used  with 
a  Householder  routine  to  compute  the  recursive  gains  which 
is  time  consuming  but  robust. 
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III.   ANALOG  SIMULATOR 

A.   GENERAL 

Simulation,  a  must  for  prototype  development,  provides  a 
source  of  numeric,  kinematic  and  logical  awareness  which  is 
vital  in  designing  a  complex  vehicle  system  and  analyzing 
its  performance.  It  provides  an  environment  in  which  the 
many  possible  states  of  the  system,  and  the  transitions 
between  them,  can  be  exercised  and  observed  in  an  accessible 
and  controlled  manner.  Properly  utilized,  simulation  will 
reduce  significantly  the  amount  of  field  testing  required. 
Design  and  performance  deficiencies  can  be  identified  early 
in  the  development  cycle,  before  the  system  is  deployed 
[Ref.  12]. 

A  technique  known  as  "Hybrid"  simulation  employs  a 
combination  of  analog  and  digital  computing  [Ref.  13]. 
Using  the  hydrodynamic  coefficients  of  the  AUV  model,  the 
analog  computer  can  be  "programmed"  to  simulate  AUV  vehicle 
dynamics.  With  the  control  program  running  and  a  "target 
depth"  entered  as  a  keyboard  input,  AUV  pitch  angle,  depth, 
and  pitchrate,  calculated  by  the  analog  computer,  are  fed 
back  to  the  digital  computer.  The  three  states  are  compared 
to  a  model  reference  and  an  appropriate  dive  plane  command 
is  determined.   The  result  is  a  continuous  update  (real-time 
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control)  of  vehicle  dive  plane  angle  (see  Figure  3.1  for 
hardware  configuration) . 

In  the  second  phase  of  program  development,  the  analog 
computer  was  configured  to  sustain  a  constant  or  variable 
bias  on  the  system.  This  lead  to  the  development  of  a  bias 
estimator. 

B.   DESCRIPTION  OF  ANALOG  SIMULATOR 

In  developing  the  analog  simulations,  the  equations  of 
motion  of  the  vehicle  are  required  to  be  converted. 
Convenient  conversion  or  scaling  is  based  on  the  maximum 
variable  values  being  scaled  with  a  +/-  10  volts  maximum 
output  on  each  amplifier.  In  analog  simulations,  variable 
summations  and  integrations  are  performed  by  summing  and 
integrating  operational  amplifiers.  Multiplication  by 
constant  coefficients  are  performed  by  attenuating  potenti- 
ometers set  to  the  appropriate  fraction  of  1.0.  With 
scaling  consistently  applied  through  the  1  or  10:1  input, 
appropriate  gains  are  sent  to  the  corresponding  amplifiers. 

Figure  3.2  shows  an  example  of  a  first  order  system 
simulation  using  an  analog  operational  amplifier.  The 
equation  is: 

q  =  [1.5]  6  rad/sec 
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(10  VOLTS  =  1  RAD) 


Figure  3.2   Example  of  a  First  Order  System 

Using  an  Analog  Operational  Amplifier 


where  6  is  in  radians.  Note  the  coefficient  1.5  rad/sec  per 
radian  is  set  at  0.15  in  the  potentiometer  with  a  gain  of  10 
in  the  integrating  amplifier. 

In  Figure  3.3,  the  diagram  represents  the  third  order 
model  of  the  vehicle  given  by  Equations  in  II. C.  Table  2.1 
gives  a  listing  of  the  system  parameter  values,  their 
dimensions,  and  the  potentiometer  settings  used  in  the 
simulations. 
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Figure  3.3   Vehicle  Simulation  Analysis 
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IV.   DIGITAL  CONTROL  PROGRAM  DEVELOPMENT 

A.   GENERAL 

As  explained  in  Reference  7,  this  digital  control 
program  represents  a  functional  component  at  the  lowest 
level  of  the  digital  autopilot  echelon.  The  digital  control 
program  interfaces  with  the  servos  controlling  the  AUV  dive 
planes.  The  "handshaking"  of  data  between  the  digital 
controller  and  the  voltage-operated  servos  onboard  the  AUV 
model  are  accomplished  by  A/D  interface  hardware  [Ref.  14]. 
In  Figure  3.1  the  hardware  arrangement  using  the  analog 
computer  as  a  simulator  of  sensor  feedback  is  exhibited. 

Much  of  the  program  development  of  Reference  7  is 
implemented  into  this  thesis.  A  number  of  dilemmas  had  to 
be  overcome  for  program  progression  to  continue. 

The  preliminary  program  code  had  parameter  conversions 
of  feet/sec,  inches,  and  degrees/sec.  To  alleviate  valuable 
computation  time,  and  to  better  support  the  AUV  model,  the 
code  for  the  input  and  ouput  (I/O)  dimensions  was  modified. 
All  I/O  parameter  dimensions  were  converted  to  voltages. 

Significant  concern  focused  on  the  programmable  inter- 
rupt control.  Previous  work  was  hampered  when  software 
interaction  was  unachievable.  The  preliminary  control 
program  would  run  only  when  initial  keyboard  inputs  of 
target   depth   were   entered.     Pre-programmed   keyboard 
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interrupt  modes  would  not  respond  to  subsequent  inputs. 
Keyboard  interaction  was  masked  and  additional  dive  plane 
commands  could  not  be  executed.  The  program  could  terminate 
only  if  the  CTRL- BREAK  key  combination  was  executed. 

From  Reference  11  it  was  discovered  that  by  removing  the 
influence  of  the  CTRL-BREAK  character  at  the  start  of  the 
programming  code,  programmed  interrupts  could  be  invoked. 

By  overcoming  this  impasse,  program  development  was 
inevitable.  Significant  algorithm  progression  followed. 
"Canned"  maneuvers,  invoked  by  keyboard  inputs,  were 
developed  and  tested.  Data  collection  and  filing  were 
implemented  by  actuating  function  key  inputs. 

The  final  phase  of  research  was  committed  to  the 
development  and  implementation  of  an  Estimator/Observer 
control  program.  This  algorithm  would  reconstruct  the  state 
variable,  pitch  angle,  using  depth  and  pitchrate  sensor 
feedback.  Bias  created  by  the  relatively  inexpensive 
onboard  sensors  was  determined  by  a  Kalman  filter  and 
removed  from  the  system.  A  cleaner,  more  robust  control 
signal  resulted. 

B.   A/D,  D/A  CONVERSION 

The  Analog  to  Digital  (A/D)  and  Digital  to  Analog  (D/A) 
conversion  process  is  vital  to  the  operation  of  this  control 
system.   Analog  sensor  signals  from  the  AUV  are  digitized 
via  the  A/D  process  prior  to  entering  the  computer  control 
program.   Likewise,  digitized  command  signals  are  converted 
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to  analog  signals  via  the  D/A  process  prior  to  return  to  the 
dive  plane  servos.  For  the  preliminary  AUV  autopilot  design 
described  in  Reference  1 ,  the  total  cycle  time  for  a 
complete  I/O  conversion  process  was  5  milliseconds  with  a  50 
millisecond  sample  rate  (20  Hz)  .  In  later  paragraphs, 
discussion  will  focus  on  the  need  to  decrease  the  sample 
rate  in  order  to  augment  the  larger,  more  complex  control 
code. 

The  digitizing  process  is  accomplished  by  the  Data 
Translation  (DT)  board,  DT  2801-A.  It  is  installed  internal 
to  the  computer  as  explained  in  Reference  7.  The  board 
configuration  can  be  found  in  the  technical  reference  manual 
[Ref.  14].  All  conversions  on  the  DT  2801-A  board  are 
controlled  by  writing  data  to,  and  reading  data  from, 
registers  on  the  board.  PCLAB  subroutines  is  an  interfacing 
software  package  that  coordinates  this  task  [Ref.  15] 

Conversion  of  a  voltage  value  to  a  digitized  value 
depends  on  the  number  of  bits  of  resolution  used  by  the 
converter.  For  instance,  a  12 -bit  converter  has  4  09  6  Number 
Of  Codes  (NOC)  .  For  a  bipolar  range  of  +/-  10  volts,  a  0 
NOC  would  correspond  to  -10  volts,  while  a  4096  NOC  would 
correspond  to  +10  volts.  An  NOC  of  2048,  similarly,  would 
correspond  to  0.0  volts. 

Before  these  digitized  voltage  values  are  usable  in  the 
software  control  program,  they  need  to  be  scaled  to  an 
equivalent  numerical  voltage  value.    Equations  found  in 
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Appendix  D  of  [Ref.  15]  were  implemented  into  the  control 
code  for  such  a  reason. 

C.   REAL-TIME  CONTROL  CODE 

The  control  program  code  was  written  in  the  programming 
language  Turbo  Pascal  version  3.0.  A  more  proficient 
version  (V4.0)  is  presently  available,  but  it  is  not 
currently  supported  by  the  interfacing  software  (PCLAB) . 

A  real-time  controller  is  "real-time"  up  to  the  limits 
of  the  software  program  implementing  it.  A  significant 
advantage  of  Turbo  Pascal  is  operation  speed.  Reducing 
large  problems  down  into  smaller,  easier  defined  procedures 
affords  faster  execution.  Each  procedure  can  then  be 
subdivided  further,  at  the  discretion  of  the  programmer. 

The  control  program  executes  an  AUV  dive  control  system 
within  a  user  friendly  multiple  menu-shell  setup.  When 
running  the  compiled  control  program,  the  first  screen 
encountered  is  the  Main  Menu.  Here,  the  user  is  presented 
with  a  display  of  program  titles  and  the  options  to  quit  or 
continue  (run) .  The  next  screen  offers  a  similar  choice, 
where  depressing  the  function  key  Fl  starts  the  control 
mode.  In  this  mode,  the  user  is  asked  to  name  a  "data  file" 
(standard  IBM  DOS  file) .  The  results  of  the  vehicle 
dynamics  during  the  course  of  the  run  will  be  stored  in  this 
file.  The  final  input  screen  prompts  the  user  to  "ENTER  THE 
TARGET   OPERATING   DEPTH."    Completion  of  this  procedure 


26 


activates  the  closed-loop  dive  plane  control.  The  Run-Mode- 
Screen  is  introduced  on  the  monitor  displaying  updated 
values  of  AUV  depth,  pitch,  pitchrate,  dive  command  angle, 
sensor  bias,  and  attitude. 

During  closed-loop  control,  the  user  has  four  methods  of 
operation: 

-  F3 — exit  from  the  "Active  Control  Mode,"  and  file  close- 
out. 

-  F2 — activates  a  pre-programmed  "Canned"  maneuver. 

-  Fl — exit  from  the  "Active  Control  Mode,"  close-out 
previously  selected  file,  creates  new  file  with  user 
interaction,  and  requests  new  target  depth  to  reactivate 
closed-loop  control. 

-  Any  alpha/numeric  key — temporarily  exits  from  "Active 
Control  Mode,"  prompts  user  for  a  new  target  depth, 
while  retaining  previously  selected  data  file  for  data 
storage. 

Two  separate  control  programs  were  used  to  establish 

data  collection  and  analysis.   The  first,  AUVC0P3F. PAS ,  was 

designed  as  a  closed-loop  dive  control  system  with  full 

state  feedback.   The  "C0P3"  corresponds  to  the  third  copy  or 

generation  control  program.    The  "F"  coincides  with  data 

Filing  capabilities.   "PAS"  is  Turbo  Pascal's  standard  file 

type  designation  for  programs  to  be  complied.   The  second 

program,  AUVC0P4F. PAS,  implements  an  Observer/Estimator  to 

reconstruct   an  unknown   state  variable   from  two   sensor 

inputs.   Sensor  Bias  is  also  estimated  and  removed  from  the 

system. 
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D.  SAMPLE  RATE  CONTROL 

A  signal  generator  excites  the  DT  2801-A  board  to  pass 
data  to  and  from  the  data  registers.  The  dial  setting  of 
the  generator  determines  the  rate  at  which  this  is 
accomplished.  As  mentioned  earlier,  a  sample  rate  of  2  0  Hz 
was  used  in  Reference  7. 

When  running  the  full-rate  feedback  control  program 
(AUVC0P3F.PAS)  at  2  0  Hz  sample  rate,  Turbo  Pascal  computed 
the  relatively  small  algorithm  in  5  milliseconds.  Manually 
reducing  the  sample  rate  during  program  runtime  resulted  in 
a  stable  controller  as  low  as  3  Hz.  An  ideal  condition, 
though,  is  to  maintain  a  high  sample  rate  to  yield  a  more 
stable,  faster  responding  controller. 

The  Estimator/Observer  control  program  (AUVC0P4F. PAS) , 
however,  contains  an  additional  400  hundred  lines  of  code. 
Here,  computation  time  was  increased  significantly  (greater 
than  150  milliseconds)  and  could  not  keep  pace  with  the  20 
Hz  sample  rate.  To  accommodate,  the  sample  rate  was  reduced 
to  5  Hz.   Likewise,  new  gains  were  derived. 

Current  hardware/software  configurations  of  the  AUV 
controller  are  limited  in  performing  these  multiple  matrix 
calculations  at  high  sample  rates. 

E.  SOFTWARE  LIMIT  FOR  ACTUATOR  SATURATION 

Present  hardware  configurations  on  the  3  0  inch  model 
required  small  input  voltages  to  actuate  the  full  range  of 
the  dive  plane  angle.   During  previous  research  of  open-loop 
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tests,  dive  plane  angle  was  compared  with  the  corresponding 
input  voltage.  Results  concluded  that  a  linear 
proportionality  on  the  order  of  0.01  volts  per  degree  dive 
plane  angle  existed.  This  was  true  for  a  range  of  0.0  to 
10.0  degrees.  Hence,  the  operating  range  of  the  dive  planes 
became  +/-  10  degrees,  and  respectively,  the  operating 
voltage  range  became  +/-  0.1  (using  the  hand-held 
transmitter).  The  dive  plane  voltage  range  of  +/-  0.1  was 
implemented  into  the  analog  computer  configuration.  The 
control  code  was  changed  by  creating  a  software  stop  prior 
to  sending  the  final  command  signal  to  the  plane.  This  was 
implemented  by  limiting  the  final  signal  sent  through  the 
D/A  conversion  to  +/-  0.1  volts  in  the  form, 

if  abs(u)  >  0.1   then   u  =  0. l*abs (u)/u. 

F.   FILES  FOR  DATA  RECOVERY 

During  the  open-loop  vehicle  tests,  data  acquisition  was 
achieved  by  using  the  software  program  DT  Notebook  [Ref.  5] 
on  the  IBM  PC/AT.  The  computer's  system  configuration  file 
had  to  be  specifically  structured  for  DT  Notebook  operation. 
However,  the  programming  language  Turbo  Pascal  used  an 
entirely  different  configuration.  The  two  programs, 
therefore,  could  not  be  run  simultaneously.  In  other  words, 
data  acquisition  by  DT  Notebook  could  not  be  performed  while 
running  the  closed-loop  control  program. 
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An  alternate  method  of  data  recovery  was  devised  using 
Turbo  Pascal  [Ref.  11].  The  keyboard  commands  for  data  file 
use  are  found  in  Chapter  IV. C.  Although  additional  through- 
put is  needed  for  data  file  simulation,  the  time  used  in 
writing  data  to  a  hard  disk  was  found  to  be  negligible  in 
comparison  to  the  sample  rate. 
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V.   CLOSED-LOOP  RESULTS 

A.   GENERAL 

In  Figure  5.1,  the  closed-loop  full  state  feedback 
system  is  displayed  in  block  diagram  convention.  It  is 
comprised  of  the  system  model: 


i 


Figure  5.1   Full  State  Feedback  Diagram 


X  =  [A]x  +  [B]u 


and  the  feedback  control: 


u  =  [N]r  -  [Kc]x 


31 


where  the  input  of  target  depth  is  the  set  point  r,  the 
closed  loop  gains  are  Kc,  and  the  setpoint  matrix  N  is 
designed  for  zero  steady  state  depth  error.  The  following 
sections  in  this  chapter  evaluate  the  response  of  the  above 
system  under  varying  conditions.  The  gains  are  fixed  and 
the  sample  rate  is  20  Hz. 

B.  RESULTS  OF  FULL  STATE  FEEDBACK 

In  the  full  state  feedback  controller,  the  closed  loop 
gains  are  designed  for  an  AUV  speed  of  2.1  Ft/s.  In  the 
following  figures,  AUV  depth  voltages  ranging  from  0.0  to 
10.0  volts  represent  the  water  depth  from  the  surface  to  a 
depth  of  10.0  feet  below  the  surface  respectively.  The  dive 
command  voltage  ranges  from  +/-  0.1  volts  equivalent  to  a 
dive  plane  angle  of  +/-  10  degrees.  In  Figure  5.2,  a  target 
depth  of  6.0  volts  becomes  the  new  set  point  from  1.0  volt. 
Subsequently,  a  dive  command  of  10  degrees  is  sent  to  the 
dive  planes.  Almost  immediately,  the  AUV  responds  and 
levels  out  at  the  new  depth.  The  entire  "run"  lasts 
approximately  12  0  sample  increments  or  six  seconds,  as 
expected,  with  an  overall  dominant  time  constant  close  to 
the  1.0  second  used  in  the  gain  design  process. 

C.  EFFECT  OF  ACTUATOR  SATURATION 

As  previously  mentioned,  the  dive  plane  actuator  is 
limited  to  a  range  of  +/-  10  degrees.  If  this  limit  is 
removed  then  the  results  of  Figure  5.3  are  found.   Comparing 
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this  to  a  second  run,  having  actuator  saturation,  the 
results  of  Figure  5.4  show  that  the  non-saturated  dive 
command  achieves  a  slightly  faster  AUV  depth  response. 
Figure  5.4  attains  5.0  volts  in  approximately  six  seconds 
whereas  Figure  5.3  attains  target  depth  in  approximately  5.5 
seconds.  In  Figures  5.5  and  5.6,  a  similar  comparison  is 
made.  Here,  however,  the  depth  change  is  considerably 
smaller.  Similarly,  the  response  time  difference  is  less 
noticeable. 

As  one  would  expect,  a  larger  actuator  in  the  dive  plane 
drives  a  quicker  AUV  response.  While  actuator  rate-of- 
change  limits  may  be  an  additional  limit  to  consider,  these 
results  assumed  that  such  a  limit  was  not  present. 

D.   EFFECT  OF  VEHICLE  SPEED 

Though  the  closed  loop  gains  were  designed  to  operate 
the  AUV  at  a  speed  of  2  FT/s,  other  speeds  were  tested  to 
determine  the  robustness  of  the  controller  so  obtained.  In 
Figures  5.7,  5.8,  and  5.9,  the  vehicle  speeds  of  two,  four, 
and  six  FT/s  respectively  were  evaluated  when  the  AUV  depth 
changed  from  six  to  four  volts.  In  Figure  5.7,  the  AUV 
smoothly  reaches  the  assigned  depth  without  overshoot  in 
approximately  five  seconds.  In  Figure  5.8,  target  depth  is 
attained  in  nearly  3.5  seconds  with  a  slight  overshoot 
before  leveling  out.  Finally,  in  Figure  5.9,  overshoot  is 
most  noticeable,  but  target  depth  is  accomplished  in  close 
to  three  seconds.   Based  on   these   observations,  it   can  be 
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noted  that  through  a  3  00%  change  in  the  speed  parameter,  the 
closed  loop  control  performance  remains  stable  with  only  a 
40%  change  in  response  time  and,  for  overshoot,  a  change  of 
less  than  15%. 
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VI.   BIAS  ESTIMATION  AND  RESULTS 

A.   GENERAL 

In  this  chapter,  the  final  stages  of  program  design  are 
implemented.  The  results  are  graphed  and  evaluated.  Here, 
the  fullstate  feedback  controller  in  Chapter  V  is  combined 
with  a  parameter  estimator  and  state  observer  system  as 
shown  in  Figure  6.1.  The  appendix  details  this  program 
code. 

The  parameter  estimator  determines  sensor  bias  of  the 
depth  pressure  cell  and  pitchrate  gyro.  The  three-state 
observer  determines  the  missing  state,  pitch  angle,  and 
filters  the  return  signal  of  the  remaining  two  states. 

With  the  inclusion  of  the  above  algorithms  (over  400 
lines  of  additional  code),  the  20  Hz  sample  rate  or  0.05 
seconds  per  sample,  had  to  be  reduced.  Significant  increase 
in  computation  time  prevented  a  sample  rate  much  higher  than 
5  Hz  or  0.2  seconds  per  sample.  Hence,  the  new  sample  rate 
became  5  Hz  and  the  poles  in  the  z-plane  were  changed 
accordingly.  The  remaining  figures  in  this  chapter  show  the 
results  of  the  new  code  at  work.  For  purposes  of 
evaluation,  vehicle  depth  change  remains  constant  while 
other  parameters  are  varied. 
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Figure  6.1   Diagram  of  the  Bias  Compensated 
Model  Based  Controller 
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B.  EFFECT  OF  SPEED  CHANGE  ON  AUV  RESPONSE 

In  Figure  6.2  at  an  AUV  speed  of  2  ft/sec,  a  smooth  dive 
maneuver  from  1.0  volt  to  5.0  volts  yields  a  response  time 
of  13  seconds,  as  compared  to  six  seconds  in  Figure  5.4. 
The  decrease  in  sample  rate  has  moved  the  poles,  retarding 
AUV  response  time  by  more  than  a  factor  of  two. 

In  Figure  6.3,  the  vehicle  speed  is  doubled  to  four 
ft/sec  and  the  same  maneuver  is  invoked.  Here  the  depth 
response  curve  is  not  as  smooth  or  direct,  but  no  overshoot 
is  observed.  Since  the  poles  are  designed  for  a  slower 
speed,  the  vehicle  tries  to  settle  out  prematurely,  delaying 
the  depth  change  process.  This  maneuver  was  accomplished  in 
17  seconds. 

Results  of  this  vehicle  speed  increased  to  six  ft/sec 
are  shown  in  Figure  6.4.  Again,  the  curve  is  not  smooth, 
with  abrupt  settling  occurring  after  two-thirds  of  the  depth 
change  and  then  continuing  on  to  5.0  volts  depth.  The 
completion  time  for  this  maneuver  is  greater  than  20 
seconds. 

For  each  increase  in  speed,  maneuver  completion  time  is 
extended,  but  target  depth  overshoot,  however,  does  not 
occur. 

C.  AUV  RESPONSE  TO  A  BIAS  SIGNAL 

As  mentioned  in  preceding  chapters,  the  return  signals 
from  the  relatively  inexpensive  sensors  onboard  the  3  0  inch 
model  are  prone  to  include  sensor  bias.   In   the   subsequent 
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figures,  the  results  of  vehicle  depth  with  sensor  bias  is 
shown.  A  0.1  volt  sensor  bias  in  Figure  6.5  results  in  a 
corresponding  0.1  volt  decrease  in  actual  depth  as  compared 
to  the  target  depth  of  five  volts.  Although  maneuver 
completion  time  is  not  influenced  (equivalent  to  Figure  6.2 
with  zero  bias) ,  the  target  depth  is  never  quite  reached. 
Similarly,  in  Figure  6.6,  a  larger  sensor  bias  of  0.2  volts 
results  in  a  vehicle  depth  falling  short  of  the  target  depth 
by  the  same  amount. 

By  slightly  adjusting  the  program  code,  the  estimated 
bias  can  be  removed  from  the  system  resulting  in  a  cleansed 
depth  response  signal,  shown  in  Figures  6.7  and  6.8. 
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VII.   SUMMARY  AND  CONCLUSION 

A .  SUMMARY 

This  thesis  presents  the  implementation  of  a  real  time 
autopilot  dive  plane  controller  for  the  NFS  AUV  testbed. 
The  controller  includes  a  state  observer  and  sensor  bias 
estimator.  The  approach  to  the  design  of  the  autopilot 
includes: 

-  Manipulation  of  vehicle  dive  dynamics  to  develop 
linearized  equations  of  motion, 

-  Implementation  of  an  Analog  Simulator  using  data  from 
open  loop  control.  This  affords  more  effective  and 
efficient  development  of  the  full  state  and  partial 
state  feedback  control  algorithms, 

-  Hardware/Software  interface  via  A/D  and  D/A  conversion 
boards, 

-  Program  development  using  Turbo  Pascal  version  3.0, 

-  Sample  rate  control  on  program  code  of  differing 
magnitudes, 

-  Incorporating  data  recovery  via  file  opening  techniques. 

B.  CONCLUSION 

This  study  developed  a  dive  plane  autopilot  for  the  NPS 
testbed  that  showed  signs  of  robustness.  Under  closed  loop 
control,  the  vehicle  stabilized  at  various  depths  under  wide 
ranges  of  speed  with  no  overshoot. 

As  program  code  increased  in  size,  throughput  increased 
reducing  sample  rate,  and  therefore,  reducing  autopilot 
response  time.    Future  research  of  the  steering  control 
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autopilot  may  need  to  optimize  current  algorithms  to  prevent 
further  throughput  increase  and  to  optimize  robustness 
characteristics.  Parallel-tasking  processors  may  be  an 
option  to  large,  complex,  programming  code,  creating  a 
faster  throughput. 
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APPENDIX 


CONTROL  CODE  FOR  AUTOPILOT 


program 


AuvAutoPilot   (  Input,  output  ); 


(  TITr,E 
AUTHOR 
APPLICATION 


DATE 


Autonomou!5  Underwater  Vehicle  Auto  Pilot  Program. 

LT  Jerry  J.  Relna  USN 

real-time  controller  with  a  Model  Based  Compensator  to 

estimate  pitch  angle,  and  a  parameter  estimator  to  determine 

sensor  bias 

8  Dec  1988 


Project  Description  :  This  program  Implements  digital  control  of  the  NPS 
autonomous  underwater  vehicle  ( AUV )  In  the  vertical  or  dlvf>  plane. It  samples 
vehicle  sensor  Input  from  two  channels  :  depth  and  pltchrate.   The  Model 
Base  Compesator  determines  pitch  angle.   All  states  are  then  multiplied  by 
the  controller  gains  and  compared  to  the  target  depth.   An  appropriate  dive 
command  Is  determined  and  sent  back  to  the  vehicle  via  D/A  conversion  boards, 
A  deflection  on  the  dive  planes  result.  The  sample  rate  is  5  Hz.) 


{  GLOBAL  DECLARATIONS 


const 
I  


Screen  declarations 


} 


xl 

yi 

x2 

y2 


5; 

2; 

75; 
24; 


Upper  left  corner 
Upper  left  corner 
Lower  rlglit  corner 
Lower  tight  corner 


left  edge  ) 
upper  edge  } 
;  right  edge  ) 
;  bottom  edge  } 


type 
strlO 
strGO 


string  110]; 
string  1601; 


var 

hr , hr  2,mi  n,ml n2, 

sec,sec2,hun,hun2 

seconds 

option,  controlmode, 

reply, reply2 


:  byte; 
:  real; 

:  char; 


{ INCLUDED  FILES  Declarations 


($1  pcldefs.tp   } 
{$1  pclerrs.pas  ) 


{  PC  I.AB  Trubo  Pascal  routines. 
{  PC  LAB  error  code  messages  file 


{ $Idrawbox2 .auv) 

(  Input  xl,yl,x2,y2  :  integer  to  specify  the  corner  limits  of  the  box, 

This  procedure  clears  screen  and  draws  a  rectangular  box  of  specified 

dimension  using  ASCII  double  line  characters. 

( $Iclrbox2 .auv) 


56 


{  Input  xl,yl,x2,y2  :  integer  to  specify  the  corner  limits  of  the  box. 

This  proce(3ure  uses  a  FAST  mefln?5  of  clearing  a  box  of  specified  dimension. 
The  box  dimension  should  be  delcared  as  constants.  ) 

( $Iboxpr Int .auv  } 

{  Input  the  prlntrow,  leftboxodge,  rightboxedge   :  integer  and  prlntstring  : 
strSO.   This  procedure  centerprlnts  the  string  in  the  box  at  the  prlntrov 
specified  without  overwriting  the  box  border.  ) 

{$ I  show fast .auv) 

{  Input  message  :  str60,  column, row  :  Integer.   To  specify  the  x,y  position 

on  the  screen  for  a  FAST  message  print.  ) 

{$Ikeyhit .auv} 

{  This  Is  a  boolean  function  which  returns  true  or  false  if  key  Is  pressed; 

it  also  returns  keycode  replies  VAR  reply,  reply2  :  char   .  ) 

{ $1 tabxy .auv) 

{  Input  tabcol , tabrow  :  Integer;  like  gotoxy  ) 

t$Iboxpause .auv) 

{  Input  xpause,ypause  :  integer  to  specify  where  "Press  any  key  to  continue" 
message  is  to  be  printed.  ) 

{ $Igetkey . auv  ) 

{  Input  as  a  string  of  chars,  the  set  of  acceptable  replies;  le  'YyNn*.   This 
procedure  waits  until  one  of  the  acceptable  replies  has  been  entered.       ) 


(  *****************  MAIN  PROGRAMS  PROCFDURES  ****♦♦****♦*******♦*♦**♦**♦*  ) 
{  ******♦**♦♦♦***♦*  USER  INTERFACE  MODULES  ****************************  ) 
procedure  MainMenu  (  var  reply  :  char  ); 

{   This  procedure  presents  the  AUV  screen  and  solicits  an  option  to  Run 

the  AUV  from  the  StatusAndCommand  procedure  or  to  Quit.  ) 

beg  In 

repeat 

clrscr ; 

drnwbox? (x3 ,yl, x2, y7 ) ; 

boxpr lnt(yl+3,xl,x2, 'N  AVAL    POSTGRADUATE    SCHOO  L'); 

boxpr Int (yl+5,xl,x2, 'D    EPARTMENT         OF'); 

boxpr int(yl+6,xl,x2, 'M  ECHANICAL    ENGINEERIN  G'); 

boxpr int (yl+8,xl,x2, 'AUTONOMOUS    UNDERWATER    VEHICLE'); 

boxpr lnt(yl no, xl,x2, 'DIGITAL    AUTOPILOT   CONTROL    PROGRAM'); 

boxprlnt(yl+12,xl  x2  '*******♦******************************' ) • 

boxpr 1 nt (yl4l 5,xl, x2, 'Do  You  want  to  RUN  this  program  ..'); 

boxpr Int (yl H6, xl, x2, ' or  Do  You  want  to  QUIT  and  return  to  DOS  ?'); 

boxpr  lnt(yH20,xl,x2,  '>>>>   ENTER  Q  OR  R   <<<<'); 

getkey  ( 'QqRr ', reply, repl y2 ) ; 
until  (  reply  In  t 'Q ' , ' q' , 'R ' , ' r ' )  )  and  (reply2  =  chr(O)); 
end; 
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procedure  StatusAndCommand  (  var  mode  :  char  ); 

I  This  procedure  begins  the  control  program   screen.  ) 

var 

mode2  :  char; 

procedure  StatusAndComm-indScreen; 

(   This  is  the  status  and  control  screen  and  solicits  a  user  input  of  Fl  to 
RUN  the  program  or  Q  to  Quit  and  exit  to  the  main  menu.  ) 

begin         {  S tatusAndCommandScr een  } 

clrbox2  ( xl, yl, x2, y2 ) ; 
boxpr int (yl H,xl,x2, 'AUV  STATUS  /  COMMAND  AND  CONTROL  SCREEN'); 
boxprint(yl+2,xl,x2, '==========================''=-===================='); 

boxpr lnt(yl+7,xl,x2, 'CHOOSE  YOUR  DESIRED  CONTROL  MODE   : ' ) ; 
boxpr int(yl+9,xl,x2, 'ENTER  KEY   <<  Fl  >>    TO  START  AUV  CONTROL'); 
boxpr int(yl+ll,xl,x2, 'ENTER   <<  Q  >>  TO  QUIT  AND  RETURN  TO  MAIN  MENU'); 
boxpr int(yl+16,xl,x2, "PRESS  EITHER    Fl    OR   Q'); 

end;  {  StatusAndCommandScr een  ) 

{  ***********************  CLOSED  LOOP  CONTROL  ROUTINES  ********************  ) 

procedure  ClosedLoopContr ol ; 

{  This  module  comprises  the  closed  loop  control  sciieme.  ) 

label  5; 

LABEL  1; 

const 

maxdepth         =  33; 

mlndepth        =  0; 

updatelncrement  =  10; 

type 

try  =  array[1..10)  of  real; 
act i vecontrolmode    =  (  run,  reset,  exit  ); 
allowabledepthrange  =  mindepth . .maxdepth  ; 

auvattitude         =  (  climb,  maintain,  diving  ); 

digitallntegerarray   =  array  tl..3]  of  integer; 

var 

filename:string[14I; 

filevarrtext; 

auvdepth,  auvdepthvolts,auvspeed,auvpltch,auvpitchvol ts , auvpitchvoltsxx, 

auvspeedvolts,  auvpitchrate,  auvpitchratevol  ts  ,e5tdept)i,  er  r  , 

deptherrorvolts,  targetdepthvolts  ,biasl, dive volts, tgtnev, targe tdepth,speed:r 
eal; 

adv  :  digitallntegerarray; 

j , status, t ime  :  integer; 

moder  eply,  moder  eply2  :  ciiar; 

actlvemode  :  act i vecontrolmode ; 

updatecounter,  initial  :-  Integer; 

depthrange  :  allowabledepthrange; 
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attitude 
acl 
nc2 
ac3 


auvatt 1 tude; 
real ; 
r  p  a  ]  ; 
real ; 


procedure  GetTar ge tDcpth  (var  tgtdepth  :  real  ; 

var  tgtdepthvolts  :  real  ); 

{  This  procedure  solicits  the  target  AUV  operating  depth  and  converts  It  to 
an  AUV  e<3ulvalent  targetdepth  analog  voltage  and  passes  both  o£  tliese 
parameters  .  ) 


hogln  {  GetTargetDepth  ) 

clrbox2  ( xl , yl, x2 , y2 ) ; 

boxpr  int  (yH10,xl,x2,  'FNTER  THE  AUV  TARGET  OPERATING  DEPTH'); 
boxpr lnt(yl+ll,xl,x2, 'NOTE  :  THE  DEPTH  SHOULD  BE  IN  VOLTS  (0.0  -  10.0)'); 
repeat 
begin 

boxprlnt  ( yl+13, xl , x2, ' ENTER  THE  TARGET  OPERATING  DEPTH  '); 
gotoxy  (xlf33,yl4l5); 
read  (  tgtdepth  ) ; 
end ; 
until   tgtdepth  >-    0.0  ; 

tgtdepthvolts  :=    tgtdepth  ; 

end;  {  GetTargetDepth ) 

procedure  RunModeScreen; 

{  This  procedure  displays  the  Closed  Loop  Control  Screen  In  the  RUN  MODE.    ) 

begin         {  RunModeScreen ) 

clrbox2  (xl , yl,x2, y2 ) ; 

boxprlnt(yl+l,xl,x2, ' A    UV         STATUS         /      CONTROL      SCREE    N') 

boxpr  lnt(y] +2,xl,x2,  •  =  =  =r^  =  =  =  =  =  =  =  =  :^  =  =  =  =  =  =  =r  =  =  =  =  =  =  =  =  =  =  =  ^  =  :=  =  =  =  =  =  =  ^  =  =  =  =  =  =  =  -= '  ); 

boxprlnt (yl+4,xl,x2, 'STATUS  OF  A  U  V  OPERATING  PARAMETERS  :'); 


write  (tabxy  ( xl+5, yl+6 ) , ' AUV  DEPTH      (volts) 
write  (tabxy  ( xl+5, yl+7 ),' AUV  PITCH      (voltsl 


write  (tabxy  ( xl+ 5, yl + 8 ) , ' AUV  PITCHRATE  (volts) 

write  (tabxy  ( xl+5, yl+9 ) , ' AUVDI VEPLANE   (volts) 
write  (tabxy  ( xl ♦ 5, yl+10 ) , ' Bl AS  =        :'); 
boxpr int(yl+ll,xl,x2, 'A  U  V   CONTROL  STATUS  :'); 

write  (tabxy  ( xl+ 5, yl +13 ),' CURRENT  TARGET  DEPTH 
write  (tabxy  ( xl +5, yl+14 ),' CURRENT  MODE 


); 
); 

); 
); 
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wrltG  (tabxy  ( xl  +  5,  yl  +  15  ),' CURRENT  MANEUVER      '.  '); 

boxpr  int(yH  18,xl,x2, 

TREGS  KEY    Fl  ..TO  ENTER  NEW  TARGET  DEPTH.         '); 
boxpr Int ( yl + 19 , xl , x2, 

'PRESS  KEY    F2  ..  TO  STOP  ACTIVE  CONTROL  AND  RESET.'); 

boxprlnt(yH20,xl,x2, 

TRESS  KEY    F3  ..  TO  EXIT  ACTIVE  CONTROL.            '); 
end;  (  RunModeScreen ) 

procedure  UpdateRunModeScreen  (updatedepth,updatepltch,updatepitchrate 
, updated Ivevolt  :  real; 

updatetargetdepth  :  real; 
upda temode  :  act  I vecontr olmode  ; 
updateatt 1 tude  :  auvattltude; 
var  blasl :  real ) ; 
{   This  module  updates  the  Closed  Loop  Control  Run  Mode  Screen  with  updated 
display  parameters.  Updates  occur  in  intervals  specified  by  update  increment 
interval  declared  in  ClosedLoopContr ol  procedure.  ) 

begin       {   UpdateRunModeScreen  ) 

(      UPDATES    STATUS  OF  A  U  V  OPERATING  PARAMETERS  ) 

writeln  (tabxy  ( xl+37 , yl +6 ) , updatedopth : G : 3 ) ; 
writeln  (tabxy  ( xl +37 , yl +7 ) , updatepl tch : 6 : 3 ) ; 
writeln  (tabxy  ( xl + 37 , yl + 8 ) , updatep 1 tchrate : 6 : 3 ) ; 
writeln  (tabxy  (xl +37, yl +9 ), updated Ivevolt : 6 : 3 ) ; 
writeln  (tabxy  ( xl  +  37, yl  +  10  ) , biasl : 8 : 6 ) ; 
(  UPDATES  THE    A  U  V   CONTROL  STATUS  ) 

write  (tabxy  ( xl+30, yl+13 ), updatetargetdepth : 6 : 2 ) ; 
case  updatemode  of 

run   :  writeln  (tabxy  (xl+ 30, yl+1 4 ) , 'RUN   '); 

reset:  writeln  (tabxy  ( xl+30, yl + 1 4 ),' RESET ') ; 

exit  :  writeln  (tabxy  (xl+ 30, yl+14 ) , ' EXIT  '); 
end; 
case  upda teatt I tudc  of 

maintain  :  writeln  (tabxy  ( xl+30, yl+15 ), 'MAINTAINING  DEPTH        '); 

climb     :  writeln  (tabxy  ( xl+30, yl+15 ),' CLIMBING  TO  TARGET  DEPTH'); 

diving    :  writeln  (tabxy  ( xl+30, yl+15 ),' DIVING  TO  TARGET  DEPTH   '); 
end; 

end;         { UpdateRunModeScreen 


procedure  GetDlgltalSensoryData  (  var  depthanalogvolts, pi tchanalogvolts, 

pitchrateanalogvolts  :real)  ; 

(   This  procedure  uses  PCLAB  routines  to  sample  selected  input  telemetry 
channels  from  the  AUV  and  digitizes  these  inputs  and  multiplies  them 
by  the  specified  gains. 

DT  2801-A  /  DT  707  Board  set  up:    channel  1   -  AUV  depth  Input 

channel  2   -  AUV  pitch  Input 
channel  3   -  AUV  pitchrate  input 
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const 


I  These  are  AUV  to  DT  2801-A  /  DT  707  hook  up  board  channel  configurations, 

conversion  and  computational  arguments.  ) 


depthchannel 

depthpfs 

depthmfs 

p 1 tchchanne  1 
spdpf s 
spdmfs 


=  1;  {  AUV  output  to  DT-707  Input  channel  assignment   ) 

=  +10.0;  {  Peak  depth  signal  value  ) 

=  -10.0;  {  Minimum  depth  signal  value  ) 

=  5;  {  AUV  output  to  DT-707  Input  channel  assignment   ) 

=  +10.0;  (  Peak  pitch  signal  value  ) 

=  -10.0;  {    Minimum  speed  signal  value  ) 


pltchratechannel  =  3;    (  AUV  output  to  DT-707  input  channel  assignment   } 


pltchratepEs   =  +10.0;   {  Peak  pltchrate  signal  value 
pitchratemfs   =  -10.0;   I  Minimum  pltchrate  signal  value 

noc  =  4096;    I  Number  of  Codes;  conversion  resolution. 

The  DT  2801-A  performs  a  12  bit  conversion, 
NOC  =  ( 2  "  conversion  bits),  le  4096 

{  SetUpAdc  and  ADConTrlgger  PCL  function  arguments 

:  p  6-8  PCL   documentation   ) 


boardnum 


=  1; 


numa2dchan     =  3; 
timingsource   =  2; 


adcga Inl 
adcga ln2 
adcgaln4 

adcga ln8 

startchannel 

endchannel 


=  1; 
=  2; 
=  4; 

=  8 
=  1 
=  3 


I  --  Sets  a  external  tr Igger, Internal  clock  ) 

{  Sets  the  A/D  gain;  1,2,4,8  are  options 


var 


pltchadv, 

depthadv, 

pi tchrateadv, 

3  igrialadv, 

counter , status , 

chanum, 1, j 


begin 


t  Signal  analog  data  value 
:  Integer; 
I  procedure  GetDlgltalTelemetry   


{  Set  up  the  DT  2801-A  board  to  take  data.  1 

status  :=  SelectBoard  (boardnum); 

{  Set  up  the  DT  2801-A  board  to  take  data  from  3  input  channels;  Data  sampling 
Is  initiated  by  the  ADConTrlgger  single  channel  sample  of  the  depth  channel 
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and  then  single  ADC  value  samples  of  the  speed  and  pJtchrate  follow. 

The  Trigger  Is  connected  to  the  DT  707  board  at  terminal  49  from  a  signal 

generating  source.  ) 


status 
status 
status 


ADConTrlgger  (  depthcliannel ,  adcgainl,  depthadv  ); 
ADCValue  (  pi  tchchannel ,  adcgainl,  pitciiadv  ); 
ADCValue  (  pitchratechannel ,  adcgainl,  pltchrateadv  ); 


{  Convert  the  digitized  Analog  Data  Values  for  pitch,  depth,  pitchrate  to 
analog  voltage  values.   The  algorithm  for  this  conversion  Is  found  in 
Appendix  D  of  the  PCLAB  documentation. 


depthanalogvolts  :=  (  depthadv  *  (depthpf s-depthmf s )/noc  ) 
pltchanalogvolts  :=  (  pltchadv  ♦  (spdpf s-spdmf s )/noc  ) 


+  depthmfs; 
+  spdmfs; 


pi tchrateanalogvolts  :=  (  pltchrateadv  * 

( pitchratepf s  -  pltchratemf s ) /noc  ) 

+  pltchratemf s; 


{    status  :=  Terminate; 
end;  {  


) 


t  PCL  routine,  closing  the  DT  2801-A 

procedure  GetDlgltalTelemetry   ) 


procedure  ATTITUDE_  (  tdepthvolts,  adepthvolts  :  real; 

var  attitude  :  auvattitude    ); 


const 


depthcontr oltolerence  =   0.1; 
modelgaln  =   1.0; 


var 


voltsdif f erence 


real; 


I  This  simulates  a  model  referenced  ) 
{   gain  parameter  ) 


begin        { Errorvolts  

voltsdif f erence  :=  tdepthvolts  -  adepthvolts; 


I  +++++++++++++++++++  Control  voltage  filter    +++++++++++++4++++++++++++4+++  ) 

(  These  conditions  check  if  depth  Is  within  tolerence .   ) 

if   (  voltsdl ff erence  >  0)  and 

(  abs(voltsdif ference )  <=  depthcontroltolerence  )  then 
begin 

attitude  :=  maintain; 
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end 
else  if   (  voltsdl £ f erence  <  0)  and 

(  abs ( vol tsd 1 f f erence )  <=  depthcontr ol tolerence  )  then 
begin 

nttltude  :=  maintain; 
end 

{  This  condition  checks  if  actual  depth  is  less  than  target  +  tolerence.) 

else  if   (  voltsd 1 £f erence  >  0)  and 

(  abs (voltsd if f erence )  >  depthcontr oltolerence  )  then 
beg  In 

attitude  :=  diving; 
end 

{  This  last  condition  checks  to  see  if  the  actual  depth  Is  more  than  target  + 
tolerence .    ) 

else  If   (  vol tsd i £ f erence  <  0)  and 

(  abs (voltsd i ff erence )  >  depthcontroltolerence  )  then 

begl  n 

attitude  :=  climb; 
end; 


end ; 


Er r orvolts 


procedure  EST(var  up, yq, z : real ; var  vhat, pitch : real ; vat  Inlt lal : Integer ; 

var  bins,al,a2,a3:  real); 
{Tills  procedure  uses  a  Model  Based  Compensator  to  determine  pitch  angle 
A  parameter  estimation  using  RLS  fit  determines  sensor  bias.) 


type 


label   3; 

try  =  array(1..10)  of  real; 
trytry  =  ar ray( 1 . . 10, 1 . . 10  )  of  real; 
try40  =  arrayd  .  .  40,1.  .  40)  of  real; 
tryt40  =  arrayll..40)  of  real; 


const 


ns 


1; 


var 


beta,g,khat,xdata,g2,phid,phiq, f c, ph 1 2, khat2, xhat2, xha t, 

g3,h3,khat3,phi,phat,xu,xy,xz,x3,xnev:  try; 

spt, £ ,spt2, f 3,5pt3  :  trytry; 

nx,np, ny,nl,nf ,n2, nsnf , time, 1 , j  :  Integer; 

2d,zf,2f2,uf,yf,fs,af, y,yhat, ud,u,yd,p0,sr3,sr2,sr,sq3,sq,sq2, 

zdd,sg2,sigmax,gbla3 :    real; 
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procedure  initial izeArrays ; 
begin 

for  1  :=  1  to  10  do 
beg  i  n 

betal 1 1 :=0.0; 
khat[ I  1 :=0.0; 
xhatt  i 1  :=  0.0; 
phl( I  1  :=  0.0; 


xull) 
xytU 
xzjl) 


=  0.0; 
=  0.0; 
=  0.0; 


end; 

np 
nx 
ny 


xnewl I  1 : =0 . 0; 


2*ns; 
np  +  1; 
1; 


y:=  0.0; 

u£:=  0.0; 

ud :=0 . 0; 

yd : =0 . 0; 

yf  :=  0.0; 

zd  : =  0.0; 

z£  :=  0.0; 

time  :=  0; 

{slgmax  =  variance  of  the  bias) 

s Igmax  : =  0,1; 

sq  :=  slgmax; 

sq2:=  slgmax; 

sq3  : =  si gmax; 

sr  : =  1.0; 

sr2  :=  1.0; 

sr3  :=  1.0; 

pO  :=  1.0e6; 


for  l! 
begl  n 


1  to  nx  do 


1  to  nx  do 


for  j:= 
begin 

spt[ 1, j )  :=  0.0; 
spt2l 1, j)  :=  0.0; 
ftl,JJ  :=  0.0; 
£3(1, jl:=0.0; 

if  1  =  j  then 

begin 

spt[ 1, j ) :=  pO; 

spt2( 1, j) :=pO; 

spt3( 1, j  J :=pO; 

f (i, Jl:=  1.0; 

end; 


e  nd ; 

gtl):=  0 
g2(il:= 


.0; 
0.0; 


end; 
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glnx  1 
g2t21 
fed 
£cl2] 
£c[3] 
fc(41 
£c[51 
fc[6] 
Ec(71 
fclB) 
£c(91 
fcllOI 


=  1.0; 
=  1.0; 

:=  2.9272E-2; 
=  -6.9013E-1; 
=  1.3633E-1; 
=  -7.556GE-2; 
=  0.0; 

=  9. 8 86  IE- 2; 
=  3.9545E-1; 
=  5.9317E-1; 
=  3.9545E-1; 
:=  9.8861E-2; ) 


Ectl)  :=0 
fc!2] :=0 
n£:=  1; 
fs  :=  5. 
£311,1): 
£3(2,11  : 

2 

3 

3 

3 

1) 

21 


80000; 
20000; 


£3[ 
£3[ 
£31 
£31 
g3( 
g3I 


21 
11 
2) 
31 


0; 

=  0, 

=  0. 

=1. 


6  9  7  7; 
1  r,  8  0  ; 

0; 

=-0.0178; 
=-0.20; 
=  1.0; 
=0. 4251; 
=0.0451; 
g3[ 3) :=-0.0031; 

khat3(ll:=-0.0073;khat3[2] 
{gains  for  5Hz ., poles? ( 0 . 75 


{discretlzed  A-matrix) 


{discretized  B-matrix) 


■0.1046;khat3l3) 
,76  0.771  kob7  } 


=  0.4250;  } 


khat3lll :=0.0;khat3(21 : =-0 . 4029 ; khat 3 [ 3  1  : =0.5677; 


{kobS 
bias  :  = 


ob8poles= ( 
0; 


71  .721  at  l£t/s) 


end  ] 


procedure  Innerprod ( var  y9 :  real;var  phi9, x9 : try; var  nx9 : Integer ) ; 


var 

j  :  Integer ; 

beg  In 
y9:=  0.0; 

for  j  :=  1  to  nx9  DO 
begin 

y9  :=  phi9ljl  *  x9lj)+  y9; 
end ; 
end; 

procedure  update(var  phi8:  try;var  uB,y8:  real;  nstinteger); 


var 


n2  :  1 nteger ; 
1 :  Integer ; 
phisave:  try; 
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3)); 
) 


begl  n 

n2:=ns*2+l; 

for  l:=l  to  n2  do 
beg  i  n 

phisavel i  J :=phl8l i  1; 
end ; 

1  : =  ns; 
while  I  >=  2  do 
begin 

phi8( n  :=  phisavel 1-11; 
phl8(ns  ♦  1)  :=  phlsave ( ns+ 1-11 ; 
1  :=  1  -  1; 
end; 

phlBlll :=  y8; 
phl8tns  +  11  :=  u8; 
end ; 

procedure  £ilter(var  u4 ,y4 : real; var  x4 , f c4 : try; var  nf 4 : Integer ) ; 
var 

nf42  :  Integer; 
savex4 : try; 

begin 

update(x4,u4,y4,nf4); 

nf42:=  2  *  nf4; 

lnnerprod(y4,x4,fc4,nf42); 
(wrlteln( ' Inside  filter  after  Inner , y4 , x4= ' , y4, x4 ( 1 1 , x4 ( 2 1 , x4 t 

end; 

procedure  newest(var  nx7:  lnteger;var   xhat7,khat7,h7 :  try; 

var    y7:real); 
var 

delta:  real; 
1, j  :  Integer ; 
xsavettry; 

begin 
for  1 : =1  to  nx7  do 
begin 

xsavel 1 ] :=xhat7l 1 1; 
end; 
delta  :=  y7; 

for  j :  =  1  to  nx7  do 
begin 

delta:=  delta  -  h7Ul  *  xhat7Ijl; 
end ; 

for  1 : =  1  to  nx7  do 
begin 

xhat7tll  :=  xsaveUl  +  khat7[ll  *  delta; 
end; 
end; 
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procedure  hholder(var  nrG,nc6:  lnteger;var  mlG:  try40); 
var 

l,col,l,j,k:  Integer; 
temp, s Igma , sum  :  real; 
d6:  tryt40;  {array  [  1  .. -10  )  of  real;) 
c6,h6 : try40; 1  atrayll . . 40, 1 , . 40 )  o£  teal;  ) 


beg  in 


begin 


for  col:=  1  to  nc6  do 

sum  :=  0.0; 

for  j  :=  col  to  nr6  do 

begin 

sum:=  sum  +  ml6 I j , col ) *ml6 ( j , col  1 ; 


d6 (k  1  )/sum; 


end; 

slgma:=  sqrt(sum); 
for  j:=  col  to  nrS  do 
beg  1  n 

d6lj) :=  0.0; 
end; 

dG ( col  1 : =  s  Igma ; 
sum  :=  0.0; 
for  j : =  col  to  nr G  do 
begin 

sum  :=  sum  +  sqr (ml6 ( j ,col 1 -d6 ( j  )  ) ; 
end ; 

for  j : =  col  to  nr 6  do 
begin 

for  k : =  col  to  nr 6  do 
begin 

begin 

1 f  abs (sum)  <  0 .00001 
thien   temp  :  =  0.0 
else  temp  :=  -2* (ml6 I j,col)-d6 ( j ) ) * (ml6 I k ,col] 

end; 

if  j  =  k  then 
begin 

temp:=  1.0  +  temp; 
end; 
h6 t  j,k ) : =  temp; 
end; 
end; 
for  j : =  col  to  nr 6  do 
begin 

for  k:=  col  to  nc6  do 
begin 

temp: =0.0; 

for  1:=  col  to  nr6  do 

begin 

temp:=  temp  +  )i6  I  j  ,  1 J  *ml6  [  l,k  ) ; 
end; 
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end ; 
end ; 
I  ml6 : =  cG; ) 


c6{ j , k 1 : =  temp; 


end; 


end; 


for  i:=col  to  nr6  do 

beg  In 

for  j  :  =col  to  nc6  do 

beg  in 

mlSIl, jl :=c6ll, jl; 

end; 

end; 

for  1 : =  2  to  nr  6  do 
begin 

for  j:=  1  to  1-1  do 

begin 

mlSll, Jl :=  0.0; 

end ; 
end; 


procedure  kgain(var  nx5:  lnteger;var  spt5,f5:  ttytry; 

var   g5, h5 : try; var  sr 5,sq5 : real ; var  khatS:  try); 
var 

ny»  1 , j , nu, k , nytot, nx5nu :  integer; 
ml5:  try40; 
sptt  :  trytry; 
begin 

inl5(l,l)  :=  sr5; 

ny:=  1  ; 

n  u  :  =  1 ; 

for  1 : =  1  to  nx5  do 

begin 

for  j:=  1  to  nx5  do 
begin 

ml5lny+l,ny+ j 1 :=  spt5[j,l); 
end; 
end; 

for  1 : =  1  to  nx5  do 
begin 

ml5lny+l,l) :=  0.0; 
for  k:=  1  to  nx5  do 
begin 

ml5lny+i,ll:=  ml5[ ny+1 , 1 ] +spt5 t k , 1 ) *h5lk ) ; 
end; 
end; 

for  j:=  ny+1  to  ny+nx5  do 
begin 

ml5ll,jj:=  0.0; 
end; 

nytot:=  ny+nx5; 
{   wrlteln  ('enter  hholder  first  time');  ) 
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(   wrlteln( 'mlS' ,ml5(l,]  ),ml5[l,2),ml5(l,31  );  ) 

hholcler(nytot,nytot,nil5); 

{  writelnl  'rrlSafter  hholder  frst  '  >  ml  5  1 1 ,  1  )  ,  ml  5  1 1 ,  2  1 ,  ml5[  1 ,  3  1) ;  ) 
(****«***♦*»*************♦*♦♦*«♦**«*****»*♦««) 

if  abs(ml5[l,  1]  )  <=  0.0000001  th-^n  writeln('R  is  singular'); 
for  I : =  1  to  nx5  do 
begin 

khat5( 1 ] :=  ml  511, 1 +  1 ]/ml5[ 1 , 11 ; 
I  wr  lteln( 'khatS^khatSl  11,  •    ',1);) 

for  j : =  1  to  nx5  do 
begin 

sptt|j,ll:=    nil5tny+i,ny+j  1; 
end ; 
end ; 

for  I : =  1  to  nx5  do 
begin 

for  j : =  1  to  nx5  do 
begin 

ml5ll, j) :=  0.0; 

for  k : =  1  to  nx5  do 

beg  in 

ml5[ 1, j 1 :=  ml5ll, jl+spttlk, i l*f 5[ j,kl; 
end; 
end; 
end; 

for  j : =  1  to  nx5  do 
begin 


end; 


ml5[nx5  +  l, j  1  :=  sq5*g5t j  1; 
end; 
nxSnu  :=  nx5+nu; 

wr iteln( 'enter  hholder  second  time');  } 
hholder(nx5nu,nx5,inl5); 
for  i : =  1  to  nx5  do 
begin 

for  j : =  1  to  nx5  do 

begin 

sptSt j,i]:=  ml5ll, jl; 

end; 
end; 


begin  ( ******beglnnlng  of  estimater »♦****) 

{Wrlteln( 'time =', time, ' in itial  =  ', Initial) ;  } 
if  initial  =  1 
then 

begin 

initialize Arrays; 
goto  3; 
end; 
ud : =uf ; 

f liter (up, uf,xu,fc,nf); 
yd  :=  y£; 
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fllter(yq,yf,xy,£c,nf); 

zdd  : =  zd; 

zd  :=  zf; 

fllter(z,zf,xz, £c,nC); 

iipdate(phl,ud,yd,ns); 

phi  Inx ) : =1 . 0; 

nsnf  :=  ns  +  n£; 

time : =t lme+1 ; 

If  time  <=  nsnf  then  goto  3; 

{  *****  estimate  bias  *****  ) 


for  j 


kgaln(nx,spt,f,g,phl,sr,sq,khat); 
{  wtlte('phl  and  yf  Into  newest ' , ph 1 ( 11 , phi [ 2 1 , ph U 3 ) , yf ) ; ) 
{  wrlte( 'khat • ,khatll),khatl 2],khatl3] );  ) 

newest (nx, xhat, khat , phi , yf ) ; 
{  wr 1 tpln ( ' paras- ' , 

xhat  in,  xhat  [2  1,  xhat  I  3  1,  xhat  I  4  l,xhatl51  )  ;  1 

(***  compute  bias  estimate  ***) 

1 : =  ns  ; 

while  1  >=  2  do 

begin 

beta[  11:=  beta[l  -  11; 
1:=  1  -  1  ; 
end; 

betalll :=  bias; 
bias : =  xhat I nx 1 ; 
Wrlteln( ' blas= ' ,blas ) ; ) 
1  to  ns  do 
begin 

blas:=  bias  +  xhat [ j 1 *beta ( j 1 ; 
end; 
al:=xhatlll;a2:=xhatt2  1;a3:=xhatI31; ) 


{*********  estimate  ********* 
phl2tll  :=  yf; 
phl2[2  1  :=  1.0; 

zf2  :=  (zf  -  2.0*zd  +  zdd ) *sqr ( f s ) ; 
n2  :=  2; 
kgaln(n2,spt2,f,g2,phl2,sr2,sq2,khat2); 

newest ( n 2, xhat 2, khat 2, phi  2, zf 2) ; 

If  abs(xhat2[ll)  >  0.00000001 
then  gblas:=  xhat2( 2 l/xhat2 ( U ; 
vhat:=  xhat2[ll; 

***  estimate  pitch  angle  *** 

f3Il,ll:=  1.0; 
f3ll,2):=  -1/fs; 
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1); 


f  3  I  2 , 1 ) : =  0.0; 

f 312,2) :=  1.0; 

g  3  111 : =  1.0; 

g3(21 :=  1.0; 

h3[l) :=  1.0; 

h  3  I  2 ) : =  0.0; 

kgain(n2,spt3,£3,g3,h3,sr3,sq3,khat3); 


) 


{observer  for  piytch  ajh, 10/2/88} 

begin 

for  1 : =1  to  3  do 

begin 

xnew[ 1 1 : =0 .0; 

for  j : =1  to  3  do 

xnew[ 11 :=xnewt  i )+f3[ 1, j l*xs[ j)+g3( 1 1 ♦up/10 . 0  +  khat 3 [ 1 1  * ( z-xs [ 3 1 +yq-xs 1 1 

end ; 

end;pltch:=xnew(2); 

xs  1 1 1 : =xnew ( 1 1 ; 

xs  I  2  1  :  =xnew[  2  1 ,: 

xs [ 3  1  : =xnewt  3  1; 

al :  =xs( 1 1  ;a2 : =xs[ 2 1 ;a3 : =xsl 31; 


3  :end; 


procedure  GenerateDlveplaneCommand  (var   auvdepth,auvpltch,auvpi tchrate, 
auvdepthcomy delta :  real;var  k:integer  ); 

{  This  procedure  takes  digitized  voltage  values  of  depth, pitch, pi tchrate, 
and  target  depth  in  volys  and  sends  new  commands  for  control.) 

const 

I  DT  2801-A  DIGITAL  TO  ANALOG  Conversion  declarations 


} 


d2achannel0  =  0; 

pfs  =  10.0; 

mf s  =  -  10.0; 

noc  =  409G; 

T=0 . 2;amp=0 . 05;T0=2 . 0;  (these  parameters  Include  sin  excitation) 

{  gl  =  0. 306 6;g2  =  0.e604;g3  =  -0. 2493;) 

(these  gains  are  for  20  Hz.?  poles=(.92  .925  .9261) 
{   gl=-0. 2286 ;g2=0. 1944 ;g3=-0. 0118; 

these  gains  are  for  6Hz.  and  poles  S  10.92  .925  .9261) 

gl=  0.2060;g2=0.6573;g3=-O.16O5; {forSHz.  at  (0..85  0.860  0.87011 


var 


dlgl taldatavalueO,  status 
el, e2, e3 


:  integer; 
:  real; 


function  ConvertAnalog2Digltal  (  analogvalue  :  real  ):  integer; 
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{  This  function  converts  analog  signal  volts  to  an  equivalent 
digital  value.   See  App  D  of  PCLAB  book.  1 

var 

temp  :  real; 

begin 

temp  :=   (  analogvalue  -  mfs  )  *  (  (noc  -  10)  /  (pfs  -  mfs  )  ); 
convertanalog2dlgl tal  : =  round  (  temp  ); 

end; 

begin   I  GenerateDl vePlaneCommand  ) 

e3:=(auvdepthcom-auvdepth) ; 

e2  :  =  (auvpltch ) ; 

el :  =  (auvpltchrate ) ; 

If    abs(e3)<=  0.000001    then  e3:=0.0; 
If  abs(el)<=0. 000005  then   el:=0.0; 
If  abs(e2)  <  =  0. 000005  then   e2:=0.0; 

delta    :=g3*e3  +  gl*(-el)  +g2*(-e2); 

if (abs(delta)>0.1)  then 
beg  In 

delta  :=0.1*abs(delta)/delta; 
end; 
delta  :=  10*delta; 
dlgltaldatavalueO  :=  convertanalog2d igltal  (  delta    ); 
status  :=  dacvalue  (  d2achannelO,  dlgltaldatavalueO  ); 
{    status  :=  terminate;    ) 

end;    {  GenerateDivePlaneCommand  ) 

procedure  MoveKvar   rO,u,r  treal); 

const 

countmax=400; 

count2=200; 

I  type 

act  1 vecontr olmode  =  ( run, reset , exit ) ; 

auvattltude  =  (cl lmb,malnta in, diving ) ;  ) 

var 

count    : Integer    ; 
dr,xl,x2,x3    :real  ; 
actlvemode  :act iveCONTROLmode; 
attitude  :auvattitude; 

begin 

act  1 vemode : =  reset; 
att  Itude : =cl Imb; 
count:=0  ; 
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end; 


while (count<countmax )    do 
begin 
dr:=0.010; 

GetDlql talSensoryData (xl,  x2,  x3 )  ; 
If  (count>couiit2  )then  dr:=0.0; 
r  :  =r 04dr  *count ; 

( I f (count>count2) then  dr:=0.0;) 

GenerateDiveplaneCommand  (xl,x2,x3,r,u, count); 
count :  =count<-l; 
(   UpdateRunModeScreen(x3,x2,xl,r ,actlvemode, attitude )   ) 
end; 


procedure  InltlallzeParameters  ; 


(  This  procedure  initializes  all  declared  control  and  display  parameters  to 
zero. 


begin 


end; 


auvpl tchvol ts  :=0.0  ; 
auvdepthvol ts  :=  0.0; 
auvspeedvolts  :=  0.0; 
auvpl tchratevolts  :=  0.0; 
auvdepth  :=  0.0; 
auvspeed  :=  0.0; 
auvpltchrate  :=  0.0; 
estdeptli :  =0  .  0; 
err  :=0  .0; 


—  procedure  InltlallzeParameters  


(  procedure  InltlallzeParameters 


begin 


Act IveControl 


inl t lallzeparameters; 

initial  :=  1;      { Initial IzeAr rays  in  EST  procedure) 

5: 

time  :=  1;         {initializes  beginning  of  data  file) 

clrscr ; 

wr lteln( 'DATA  FILE  NAME?  (ie  f082201.dat)'); 

r eadln( £  llename ) ; 

asslgn(fllevar, file name); 

revr I te ( f 1 levar ) ; 

clrscr ; 

actlvemode  ;=  run; 


repeat 


{ 


Repeat  until  actlvemode  =  exit 


{  ClosedLoopControlScreen;  ) 

GetTargetDepth  (  targetdepth,  targetdepthvolts  ); 
RunModeScreen; 
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1: 

while  (  not   keyhlt  (  modereply,  modereply2))    do 
begin 

iipdatecounter  :=  0; 

while  (  updatecounter  <   update  Increment  )  do 
begin 
GetDlgltalSensoryData  (auvdepthvolts,auvpltchvoltsxx,auvpltchratevolts); 

GenerateDlveplaneCommand  (  ac3,ac2,acl, 

targeldcpthvoltSjdlvevolt? , time  ); 
EST  (dlvevolts,auvpltchratevolts,auvdepthvolts, 

speed, auvpltch volts, Initial, blasl,acl,ac2,ac3) ; 
wr lteln( Ellevar, t Ime : 5,auvdepthvolts : 12 : 6,auvpltchvolt5xx : 12 : 6, 
auvpltchratevolts:12:6,auvpltchvolts:8: 3,divevolts:12:6, 
bla3l:12:6,ac 1:12:6, ac2:12:6,ac 3:12:6); 

ATTITUDE_  ( targetdcpthvolts,auvdepthvolts ,att Itude  )  ; 
Initial  :=  0; 
updatecounter  :=  updatecounter  +  1; 
time  :=  time  +  1; 
end;    {  while  updatecounter  <  update  Increment   ) 

UpdateRunModeScreen  (  auvdepthvolts ,  auvpl tchvol tsxx, 

auvpltchratevolts,dlvevolts,targetdepth,  actlvemode,  att Itude, blasl); 

end;  {  while  not  KeyHlt  ) 

if  (ord(inodereply)  =  27)  and  ( ord  (modereply2 )  =  59)  then   {  KeyHlt  =  Fl  ) 
begin 

close ( f llevar ) ; 
goto  5; 
end 

else  If  ( ord (modereply )  =  27)  and  ( ord (modereply2 )  =  60)  then 
begin 

actlvemode  :=  reset; 

Mo vel( targe tdepthvolts,dlve volts, tgtnew); 

targetdepthvolts : =tgtnew   ; 

GOTO  1 

end 

else  If  ( ord (modereply )  =  27)  and  ( ord (modereply2 )  =61)  then 
begin 

close ( f  llevar  ) ; 
actlvemode  :=  exit; 
end  I  KeyHlt=  f3) 

until    (act lvemode=exl t ) 

end;  {  Act IveControl    ) 
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begin         {  StatusAndCommand  -- 

repeat 

S tat us AndCommgnd Set een ; 
GetKey  { ' ' ,mode , mode2 ) ; 

i£  (  ord  (mode)  =  27  )  and  (  ord  (inode2)  =  59  )  then 
begin 

clrbox2  ( xl , yl , x2,y2 ) ; 
ClosedLoopControl; 
end ; 
until  (  mode  In  [ 'Q' , 'q' )  ) ; 


end;  {  StatusAndCommand 


procedure  InltlalizeZeroDlgltalSlgnalOut; 

{  This  procedure  MUST  be  executed  as  the  first  procedure  called  in  the  main 
program  to  insure  a  zero  signal  out  on  the  2  output  channels.    Otherwise  the 
DT  2801-A  board  defaults  to  a  minimum  full  scale  output.  ) 

const 

dlgitalchanO  =  0; 
digltalchanl  =  1; 
d igltalcommandboard  =  1; 

var 

status, 

digitaldatavalue     :  Integer; 

beg  in 

digitaldatavalue  :=  2048;       {  This  will  be  converted  to  an  equivalent 

zero  analog  signal  out  on  a  12  bit 
resolution  converter  like  DT  2801-A.      ) 

status  :=  initialize; 

status  :=  selectboard  (  digl talcommandboard  ); 

status  :=  dacvalue  (  dlgitalchanO,  digitaldatavalue  ); 

status  :=  dacvalue  (  digltalchanl,  digitaldatavalue  ); 

status  :=  terminate; 
end; 

procedure  Deact i vateADBoardAndExi tProgram; 

{  Tills  procedure  deactivates  the  DT  2801-A  board  and  presents  an  exit  screen.) 

var 
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status  :  integer; 

begin  {    Deact IvateADBoardAndExitProgram  ) 

status  :=  terminate; 

clrbox2  (xl , yl, x2,y2  )  ; 

boxprint  ( yl  +  10, xl, x2 ,  ' THIS  CONCLUDES  YOUR  AUV  AUTOP I  LOTTING  SESSION  ,  BYE'); 

end;    {  Deact IvateADBoardAndExltPr ogram  ) 


BEGIN  {  ■ MAIN  PROGRAM 

InitlallzeZeroDigitalSlgnalOut; 

clrscr ; 

repeat 

MainMenu  (  option  ); 

If  (  option  in  [  'R'  ,  'r  '  1  )  then 
begi  n 

repeat 
begin 

StatusAndCommand  (  controlmode  ); 
end; 
until  (  controlmode  in  t'q','Q'l); 
end; 
until  (  option  In  [ 'Q', 'q'  1  ); 

Deact IvateADBoardAndExltPr ogram; 

END.  I  MAIN  PROGRAM 


76 


LIST  OF  REFERENCES 


1.  5th  International  Symposiuin  on  Unmanned  Untethered 
Submersible  Technology,  University  of  New  Hampshire, 
June  22-24,  1987,  Symposium  Proceedings. 

2.  Healey,  A.J.,  Cristi,  R.  ,  Smith,  D.L.,  McGhee,  R.B., 
"Navigation,  Path  Planning,  Dynamics  and  Control  of 
Generic  Autonomous  Underwater  Vehicles  Proposal," 
Research  proposal  to  Naval  Postgraduate  School  Direct 
Research  Fund,  Monterey,  California,  April  1988. 

3.  Boncal,  R.J.,  A  Study  of  Model  Based  Maneuvering 
Controls  for  Autonomous  Underwater  Vehicles,  Master's 
Thesis,  Naval  Postgraduate  School,  Monterey,  California, 
December  1987. 

4.  Brunner,  G.M.,  Experimental  Verification  of  AUV 
Performance,  Master's  Thesis,  Naval  Postgraduate  School, 
Monterey,  California,  March  1988. 

5.  DT/Notebook ,  Laboratory  Technologies  Corporation, 
Wilmington,  Massachusetts,  1986. 

6.  MATRIXx ,  Integrated  System  Inc.,  Santa  Clara, 
California,  1987. 

7.  Delaplane,  S.W.,  Preliminary  Design  and  Cycle 
Verification  of  a  Digital  Autopilot  for  Autonomous 
Underwater  Vehicles,  Master's  Thesis,  Naval  Postgraduate 
School,  Monterey,  California,  March  1988. 

8.  D'azzo,  J.J.  and  Houpis,  C.H.,  Linear  Control  System 
Analysis  and  Design  Conventional  and  Modern,  3rd  Ed. , 
McGraw-Hill,  1988. 

9.  Turbo  Pascal  Tutor,  Borland  International,  Scotts 
Valley,  California,  1987. 

10.  Turbo  Pascal  4.0.  Borland  International,  Scotts  Valley, 
California,  1987. 

11.  Swan,  T.,  Mastering  Turbo  Pascal.  Hayden  Books, 
Indianapolis,  Indiana,  1987. 


77 


12.  Johnstone,  R.S.,  Fries,  D.W. ,  "Simulation  of  a  Submerged 
Autonomous  Vehicle  with  Inertial  Navigation,"  Paper, 
Advanced  Marine  Systems--Lockheed  Missiles  and  Space 
Company,  Inc.,  Sunnyvale,  California,  presented  at 
DARPA/CSDL  Symposium,  June  1988. 

13.  Cannon,  R.H.  Jr.,  Dynamics  of  Physical  Systems.  McGraw- 
Hill,  New  York,  1967. 

14.  User  Manual  for  DT  2801  Series,  Single  Board,  Analog  and 
Digital  I/O  Systems  for  the  IBM  PC,  Data  Translation, 
Inc.,  Marlborough,  Massachusetts,  1986. 

15.  User  Manual  for  PCLAB,  Sp041  v2.00.  Data  Translation, 
Inc.,  Marlborough,  Massachusetts,  1986. 


78 


INITIAL  DISTRIBUTION  LIST 


No.  Copies 


1.  Defense  Technical  Information  Center  2 
Cameron  Station 

Alexandria,  Virginia   22304-6145 

2.  Library,  Code  0142  2 
Naval  Postgraduate  School 

Monterey,  California   93943-5002 

3.  Chairman,  Code  69Hy  5 
Department  of  Mechanical  Engineering 

Naval  Postgraduate  School 
Monterey,  California   93943-5000 

4.  Professor  D.L.  Smith,  Code  69Sm  1 
Department  of  Mechanical  Engineering 

Naval  Postgraduate  School 
Monterey,  California   93943-5000 

5.  Professor  R.  McGhee,  Code  52Mz  1 
Department  of  Computer  Science 

Naval  Postgraduate  School 
Monterey,  California   93943-5000 

6.  Professor  R.  Christi,  Code  62Cx  1 
Department  of  Electrical  and  Computer 

Engineering 
Naval  Postgraduate  School 
Monterey,  California   93943-5000 

7.  Dr.  G.  Dobeck,  Code  4210  1 
NCSC 

Panama  City,  Florida   32407-5000 

8.  Russ  Werneth,  Code  U2  5  1 
Naval  Surface  Weapons  Center 

White  Oak,  Maryland   20910 

9.  Paul  Heckman,  Code  94  3  1 
Head,  Undersea  AI  &  Robotics  Branch 

Naval  Ocean  System  Center 
San  Diego,  California   92152 

10.  Dr.  D.  Milne,  Code  1563  1 
DTRC,  Carderock 

Bethesda,  Maryland   20084-5000 


79 


11.  Naval  Sea  Systems  Command 

Code  PMS-3  50,  Attn:   Ms.  Judy  Rumsey 
Washington,  D.C.   20361-5101 

12.  LT  Relle  L.  Lyman,  Jr. 
Navsea  Robotics  Office 
Naval  Sea  Systems  Command 
Washington,  D.C.   20362-5101 

13.  Distinguished  Professor  G.  Thaler,  Code  62Tr 
Department  of  Electrical  and  Computer 

Engineering 
Naval  Postgraduate  School 
Monterey,  California   93943-5000 

14.  LT  Gerard  J.  Reina,  USN 
107  Seafoam  Avenue 
Monterey,  California   93940 

15.  CDR  Gordon  Mac  Donald 
1  Shubrick  Road 
Monterey,  California   93940 


80 


i:^/-  9^7 


Thesis 
R329715 

c.l 


Reina 

AUV  dive  control  sys 
tern  development  includ- 
ing sensor  bias  compen- 
sation and  parameter 
estimation. 


1 


Thesis 

R3297i5 

c.l 


Reina 

AUV  dive  control  cy"!- 
tem  development  includ- 
ing sensor  bias  compen- 
sation and  parameter 
estimation. 


^"•'-oSj 


r^ 


g^.i.: 


